/* Include */
#include "HAL.h"

/* Define */
#define Servo_baudRate 1000000
#define Servo_txPin    33
#define Servo_rxPin    32

/* Servo class */
SCSCL ServoSCS;
SMS_STS ServoSM;


/* Servo driver init */
void HAL_ServoDriver_init()
{
    /* Servo serial port begin */
    Serial2.begin(Servo_baudRate, SERIAL_8N1, Servo_rxPin, Servo_txPin);
    /* Servo class setup */
    ServoSCS.pSerial = &Serial2;
    ServoSM.pSerial = &HAL_RS485;

    /* Send test */
    for (int i = 0; i < 3; i++) {
        Serial2.println("A");
        delay(200);
    }
    
    HAL_LOG.println("[HAL] HAL_ServoDriver initialized");
}
